//一小米-----------------------id:0x7F        can1   mode_angle-----炮头
//一2006-----------------------id:1           can2   mode_angle-----三叶
//两减3508---------------------id:2，6        can2   mode_speed-----摩擦轮
//一6020-----------------------id:2           can1   mode_angle-----拨盘
//一减3519---------------------id:0x03        can1   mode_angle-----铲子
//六775------------------------id:**          PWM    **********-----轮子

#include "Recangh.h"

#define Stop_Acction 0.0f
#define Switch_Up 1.0f
#define Switch_Mid 3.0f
#define Switch_Dow 2.0f

//底盘驱动的
#define PWM_ARR 2000
#define PWM_Zero 0

#define Assist_load_Dece_Ra 36  //2006的减速比
#define Rc_Para_Max 660.0f      //传数据包最大值

//转角的最小单元
#define Shovel_30 ((30.0f / 180.0f) * PI)
#define Turntable_90 ((90.0f / 180.0f) * PI)
#define Assist_load_120 ((120.0f / 180.0f) * PI)
#define Raise_Head_30 ((30.0f / 180.0f) * PI)

INTF_Motor_HandleTypeDef *Shovel;
INTF_Motor_HandleTypeDef *Assist_Load;
INTF_Motor_HandleTypeDef *Turntable;
INTF_Motor_HandleTypeDef *Surgewheel_1;
INTF_Motor_HandleTypeDef *Surgewheel_2;
INTF_Motor_HandleTypeDef *Raise_Head;
osThreadId Recangh_MainLoopTaskHandle;
static RC_ctrl_t *Rc_Ctrl;

typedef struct
{
    uint16_t Turn_R;
    uint16_t Turn_L;
    uint16_t Str;
}Wheel_Speeds;

typedef struct
{
    float Out_Speed;
}Shovel_Speed;

float Smooth_steer(float Input, float Exponent)
{
    return copysign(pow(fabs(Input), Exponent), Input);
}

void Deadzone_Pro(float* Value, float Dead_P)
{
    if(fabs(*Value) < Dead_P)
        *Value = 0.0f;
    else
        *Value = copysign((fabs(*Value) - Dead_P) / (1 - Dead_P), *Value);
}

static float Speed_Filter = 0.0f;
static const float LPF_Factor = 0.006f;
Wheel_Speeds Cal_Wheel_Speeds(int16_t L1, int16_t Di)
{
    Wheel_Speeds Speeds = {0};
    static float Direction_R_Last = PWM_ARR;
    static float Direction_L_Last = PWM_ARR;
    float Cliffy = 1.3f;
    if(L1 == 0)//转弯
    {
        if(Di > 0){Speeds.Turn_L = PWM_ARR;Speeds.Turn_R = PWM_Zero;}       //右转
        else if(Di < 0){Speeds.Turn_L = PWM_Zero;Speeds.Turn_R = PWM_ARR;}  //左转
        else if(Di == 0){Speeds.Turn_L = Direction_L_Last;Speeds.Turn_R = Direction_R_Last;}
        Speeds.Str = Smooth_steer(fabs(Di / Rc_Para_Max * PWM_ARR), Cliffy) / 6;
        Speeds.Str = (Speeds.Str >= PWM_ARR / 3) ? PWM_ARR / 3 : Speeds.Str;
    }
    else if(L1 != 0)//直行
    {
        if(L1 > 0){Speeds.Turn_L = PWM_ARR;Speeds.Turn_R = PWM_ARR;}        //前进
        else if(L1 < 0){Speeds.Turn_L = PWM_Zero;Speeds.Turn_R = PWM_Zero;} //后退
        Speeds.Str = Smooth_steer(fabs(L1 / Rc_Para_Max * PWM_ARR), Cliffy);
    }
    Speed_Filter = LowPassFilter(Speeds.Str, Speed_Filter, LPF_Factor);
    Speeds.Str = Speed_Filter;
    Speeds.Str = (Speeds.Str >= PWM_ARR * 32 / 100) ? PWM_ARR * 32 / 100 : Speeds.Str;
    Direction_L_Last = Speeds.Turn_L;
    Direction_R_Last = Speeds.Turn_R;
    return Speeds;
}

Shovel_Speed Cal_Shovel_Angle(int16_t R1)
{
    Shovel_Speed Speed = {0};
    float Dead_P = 0.05f;
    float Shovel_Cliffy = 0.9f;
    Speed.Out_Speed = (R1 / Rc_Para_Max) * 10.0f;
    Deadzone_Pro(&Speed.Out_Speed, Dead_P);
    return Speed;
}

// Shovel_Speed Cal_Shovel_Angle(int16_t R1)
// {
//     Shovel_Speed Speed = {0};
//     float Dead_P = 0.05f;
//     float Shovel_Cliffy = 0.9f;
//     uint8_t Flag = 0;
//     //500 - 1500反转      1500 - 2500正转
//     if(Flag == 0)
//     {
//         Speed.Out_Speed = 1500 + (R1 / Rc_Para_Max) * 1000;
//         Flag ++;
//     }
//     else
//         Speed.Out_Speed = Smooth_steer(1500 + (R1 / Rc_Para_Max) * 1000, Shovel_Cliffy);

//     Deadzone_Pro(&Speed.Out_Speed, Dead_P);
//     return Speed;
// }

void Recangh_MainLoop()
{
    osDelay(3000);
    //500HZ
    __HAL_TIM_SET_COUNTER(&htim8, 84-1);
    __HAL_TIM_SET_AUTORELOAD(&htim8, PWM_ARR-1);

    __HAL_TIM_SET_PRESCALER(&htim3, 72 - 1);
    __HAL_TIM_SET_AUTORELOAD(&htim3, 20000 - 1);

    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
    while(true)
    {
        //轮子
        Wheel_Speeds Speeds = Cal_Wheel_Speeds(Rc_Ctrl[0].rc.rocker_l1, Rc_Ctrl[0].rc.dial);
        __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, Speeds.Turn_R);
        __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, Speeds.Turn_L);
        __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_3, Speeds.Str);

        //铲子
        Shovel_Speed Speed = Cal_Shovel_Angle(Rc_Ctrl[0].rc.rocker_r1);
        Shovel->target_speed = -Speed.Out_Speed;
        // __HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, Speed.Out_Speed);

        //发弹
        static uint8_t Fire_Flag = 0;
        float Fire_Tick = 1.25f;      //连发间隔
        float Surgewheel_Speed = 45.0f;                         //摩擦轮转速
        float Assist_Load_P = 0.99945f;
        float Raise_Head_P = 1.0f;
        static uint8_t Fire_Num;
        static uint8_t Fire_Turn, Fire_Now, Fire_Last;
        static uint32_t Lask_Tick;
        uint32_t Cp_Lask_Tick = Lask_Tick;
        float Fire_Dt = DWT_GetDeltaT(&Cp_Lask_Tick);
        if(Rc_Ctrl[0].rc.switch_right == Switch_Up && Fire_Dt >= Fire_Tick)      //连发
        {
            Fire_Num ++;
            DWT_GetDeltaT(&Lask_Tick);
            Assist_Load->target_angle = Fire_Num * Assist_load_120 * Assist_load_Dece_Ra * Assist_Load_P;

            if(Fire_Now - Fire_Last == 2)
            {
                Fire_Turn ++;
                Turntable->target_angle = Fire_Turn * Turntable_90;
                Fire_Last = Fire_Now;
            }
            Fire_Now = Fire_Num;
            Fire_Flag = 1;
        }
        else if(Rc_Ctrl[0].rc.switch_right == Switch_Mid)
        {
            if(Fire_Flag == 1)
            {
                Fire_Num --;
                Assist_Load->target_angle = Fire_Num * Assist_load_120 * Assist_load_Dece_Ra * Assist_Load_P;
                Fire_Flag = 0;
            }
            else
                Fire_Flag = 0;
        }

        // if(Rc_Ctrl[0].rc.switch_right == Switch_Up && Fire_Dt >= Fire_Tick)     //单发
        // {
        //     Fire_Flag = (Fire_Flag == 0) ? 1 : (Fire_Flag == 1) ? 2 : (Fire_Flag == 2) ? 3 : 4;
        //     if(Fire_Flag == 1 || Fire_Flag == 2)
        //     {
        //         Fire_Num ++;
        //         DWT_GetDeltaT(&Lask_Tick);
        //         Assist_Load->target_angle = Fire_Num * Assist_load_120 * Assist_load_Dece_Ra * Assist_Load_P;
        //         if(Fire_Now - Fire_Last == 2)
        //         {
        //             Fire_Turn ++;
        //             Turntable->target_angle = Fire_Turn * Turntable_90;
        //             Fire_Last = Fire_Now;
        //         }
        //         Fire_Now = Fire_Num;
        //     }
        //     else if(Fire_Flag == 3)
        //     {
        //         Fire_Num --;
        //         Assist_Load->target_angle = Fire_Num * Assist_load_120 * Assist_load_Dece_Ra * Assist_Load_P;
        //     }
        // }

        //炮头
        if(Rc_Ctrl[0].rc.switch_left == Switch_Dow)         //一档
            Raise_Head->target_angle = -((Rc_Para_Max * 1 / 5 / Rc_Para_Max) * Raise_Head_P) * Raise_Head_30;
        else if(Rc_Ctrl[0].rc.switch_left == Switch_Mid)    //二档
            Raise_Head->target_angle = -((Rc_Para_Max * 2 / 5 / Rc_Para_Max) * Raise_Head_P) * Raise_Head_30;
        else if(Rc_Ctrl[0].rc.switch_left == Switch_Up)     //三档
            Raise_Head->target_angle = -((Rc_Para_Max * 3 / 5 / Rc_Para_Max) * Raise_Head_P) * Raise_Head_30;

        float Target_Speed = Surgewheel_Speed;
        if(Rc_Ctrl[0].rc.switch_right == Switch_Up)
            Target_Speed = Surgewheel_Speed;
        else if(Rc_Ctrl[0].rc.switch_right == Switch_Mid)
            Target_Speed = Surgewheel_Speed;
        else if(Rc_Ctrl[0].rc.switch_right == Switch_Dow)
            Target_Speed = Stop_Acction;

        Surgewheel_1->target_speed = -Target_Speed;
        Surgewheel_2->target_speed = -Target_Speed;

        //显示曲线
        USB_printf("a:%.4f,%.4f\n", Raise_Head->real_angle, Raise_Head->target_angle);
        osDelay(1);
    }
}

void Recangh_Init()
{
    Shovel = Bus_SharePtr("/motor/Shovel", sizeof(INTF_Motor_HandleTypeDef));

    Surgewheel_1 = Bus_SharePtr("/motor/Surgewheel_1", sizeof(INTF_Motor_HandleTypeDef));
    Surgewheel_2 = Bus_SharePtr("/motor/Surgewheel_2", sizeof(INTF_Motor_HandleTypeDef));
    Raise_Head = Bus_SharePtr("/motor/Raise_Head", sizeof(INTF_Motor_HandleTypeDef));
    Turntable = Bus_SharePtr("/motor/Turntable", sizeof(INTF_Motor_HandleTypeDef));
    Assist_Load = Bus_SharePtr("/motor/Assist_Load", sizeof(INTF_Motor_HandleTypeDef));

    Rc_Ctrl = Bus_SharePtr("DR16", sizeof(RC_ctrl_t) * 2);

    osThreadDef(Recangh_MainLoopTask, Recangh_MainLoop, osPriorityLow, 0, 2048);
    Recangh_MainLoopTaskHandle = osThreadCreate(osThread(Recangh_MainLoopTask), NULL);
}

//激活代码

//老的操控方式
        // if(Rc_Ctrl[0].rc.switch_left == Switch_Dow)
        // {
        //     Shovel_Speed Speed = Cal_Shovel_Angle(Rc_Ctrl[0].rc.rocker_l1);
        //     Shovel->target_speed = Speed.Out_Speed;
        // }



        // if(Rc_Ctrl[0].rc.switch_left == Switch_Up)
        // {
        //     // if(Rc_Ctrl[0].rc.switch_right == Switch_Up && Fire_Dt >= Fire_Tick)      //连发
        //     // {
        //     //     Fire_Num ++;
        //     //     DWT_GetDeltaT(&Lask_Tick);
        //     //     Assist_Load->target_angle = Fire_Num * Assist_load_120 * Assist_load_Dece_Ra * Assist_Load_P;

        //     //     Surgewheel_1->target_speed = Surgewheel_Speed;
        //     //     Surgewheel_2->target_speed = Surgewheel_Speed;

        //     //     if(Fire_Now - Fire_Last == 2)
        //     //     {
        //     //         Fire_Turn ++;
        //     //         Turntable->target_angle = Fire_Turn * Turntable_90;
        //     //         Fire_Last = Fire_Now;
        //     //     }
        //     //     Fire_Now = Fire_Num;
        //     // }
        //     if(Rc_Ctrl[0].rc.switch_right == Switch_Up && Fire_Dt >= Fire_Tick)     //单发
        //     {
        //         Fire_Flag = (Fire_Flag == 0) ? 1 : (Fire_Flag == 1) ? 2 : 3;
        //         if(Fire_Flag == 1 || Fire_Flag == 2)
        //         {
        //             Fire_Num ++;
        //             DWT_GetDeltaT(&Lask_Tick);  
        //             Assist_Load->target_angle = Fire_Num * Assist_load_120 * Assist_load_Dece_Ra * Assist_Load_P;

        //             Surgewheel_1->target_speed = Surgewheel_Speed;
        //             Surgewheel_2->target_speed = Surgewheel_Speed;

        //             if(Fire_Now - Fire_Last == 2)
        //             {
        //                 Fire_Turn ++;
        //                 Turntable->target_angle = Fire_Turn * Turntable_90;
        //                 Fire_Last = Fire_Now;
        //             }
        //             Fire_Now = Fire_Num;
        //         }
        //     }
        //     // 控制炮头的上下移动，通过控制右遥感实现
        //     if(Rc_Ctrl[0].rc.rocker_r1 < -265.0f)
        //         Raise_Head->target_angle = -((-265.0f / Rc_Para_Max) * Raise_Head_P) * Raise_Head_30;
        //     else
        //         Raise_Head->target_angle = -((Rc_Ctrl[0].rc.rocker_l1 / Rc_Para_Max) * Raise_Head_P) * Raise_Head_30;
        // }

        // if(Rc_Ctrl[0].rc.switch_right == Switch_Dow || Rc_Ctrl[0].rc.switch_left == Switch_Mid || Rc_Ctrl[0].rc.switch_left == Switch_Dow)
        // {
        //     Fire_Flag = 0;
        //     Surgewheel_1->target_speed = Stop_Acction;
        //     Surgewheel_2->target_speed = Stop_Acction;
        // }